function [cc,R,T,K,camAtWC,camLookAtWC] = CalibrateCameraNonLinear( Xreal, xImg, Zguess, Cam0 )

dbstop if error

R = zeros(3,3);
T = zeros(3,1);
K = zeros(3,3);
cc = zeros(3,4);

N = size(xImg,2);
M = zeros(2 * N, 11);
xobs = xImg(1,:);
yobs = xImg(2,:);
xx = upsample(xobs,2);
yy = upsample(yobs,2);
yy(2:end) = yy(1:end-1); yy(1) = 0;
RHS = xx' + yy';

global RHSimg;
RHSimg = RHS;
global X3Dpts;
X3Dpts = Xreal;
global depth0; % one known depth value
depth0 = Zguess;

cc = CalibrateCameraAffine(Xreal, xImg );
global DLTcc;
DLTcc = cc;

camAViewEST = GetCameraS03MatrixOGL( Cam0, [0,0,0],'init guess (Nonlinear Opt)',1) ;
R_Est = camAViewEST(1:3,1:3);
estsum = sum(sum(abs(camAViewEST(1:2,1:3))));
dltsum = sum( abs(cc(1:3)) + abs(cc(4:6)) );
fs0 = dltsum / estsum;
omegahat0 = logm(R_Est);
omega0 = [omegahat0(3,2); omegahat0(1,3); omegahat0(2,1)];

omega = omega0;
fs = fs0;
x0 = [fs; omega0(:)];

% set some options
options = optimset('Display','iter','LargeScale','off'); % must supply analytic gradient to use 'large scale'
% do the optimization
[x,fval,exitflag,output,grad,hessian] = fminunc( @costFuncAffineCam2,x0,options);

w = x(2:4);
what = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0];
R = expm(what);
K = [ [eye(2,2)*x(1) [0;0]]; [0 0 1] ];
T = [cc(4); cc(8); Zguess*x(1)] / x(1);

[camA camAtWC camLookAtWC] = DrawAxesRT('converged cam (Nonlinear Opt)',1,R,T);

breakhere = 1;

